package pteam;

import java.util.Stack;

import simulator.Actor;
import simulator.Robot;
import simulator.Sensor;

public class RouteActor implements Actor{

	public static final int FRONT_THRESHOLD = 10;
	public static final int INTERSECTION_WIDTH = 32;
	public static final double SIDE_THRESHOLD = 30;
	public static final int ADJUST_ANGLE = 10;
	public static final int DRIVE_DISTANCE = 1;
	public static final int TRIGGER_CONDITION = 2;
	public static final int START_DIRECTION = Intersection.EAST;
	private boolean atIntersection;
	private boolean driving;
	private int directionFacing;
	private double lastLeftReading;
	private double lastRightReading;
	private double lastFrontReading;
	private int triggerCount;
	private int currentWaypoint;
	//private boolean isRetracing;
	private int[] path;

	public RouteActor(String[] route){
		path = new int[route.length];
		for(int i=0; i<route.length; i++){
			path[i] = Intersection.directionFromString(route[i].split(" ")[2].trim());
		}
		directionFacing = START_DIRECTION;
		atIntersection = false;
		driving = true;
		triggerCount = 0;
		currentWaypoint = 0;
	}

	public String getMaze() {
		return "Super";
	}

	public void act(Robot r) {
		if(atIntersection && triggerCount >= TRIGGER_CONDITION){
			System.out.println("<!/>");

			Intersection i = thisIntersection();
			if(i.getWays() == 0){
				System.out.println("<dead end/>");
				turnAround(r);
				return;
			}else if(i.getWays() == 1){  //If theres only one route ahead
				if((i.getOpenDirections()[Intersection.NORTH] 
				                          && i.getOpenDirections()[Intersection.SOUTH]) 
				                          || (i.getOpenDirections()[Intersection.WEST] 
				                                                    && i.getOpenDirections()[Intersection.EAST])){
					//if that route's straight ahead, just drive
					System.out.println("<ignore/>");
					drive(r);
					//driving = true;
				}else{ //otherwise turn into doorway here
					System.out.println("<one-way/>");
					dealWithIntersection(r, i);
					driving = false;
					return;
				}
			}/*else if(this.isRetracing){
				retrace(r);
				driving = false;
				return;*/
			else{
				System.out.println("<decision point/>");
				dealWithIntersection(r, i);
				driving = false;
				return;
			}
		}else if(triggerCount > 0){
			r.goForward();
			r.waitDistance(1);
		}
		else{
			System.out.println("<o/>");
			drive(r);
			//driving = true;
		}
	}

	public boolean isDeadEnd(double left, double right, double front){
		return (front < INTERSECTION_WIDTH &&
				right < SIDE_THRESHOLD &&
				left < SIDE_THRESHOLD);
	}

	private void dealWithIntersection(Robot r, Intersection i){
		chooseDirection(r, i);
		atIntersection = false;
	}

	private Intersection thisIntersection(){
		//if when the robot is correctly oriented, two sensors pointed
		//in different directions both register sufficiently large 
		//readings, return true.
		//so the sensors are on the side? For now
		//otherwise, return false.
		int ways = 0;
		System.out.println("FINDING INTERSECTION...LEFT: " + lastLeftReading 
				+ " RIGHT: " + lastRightReading + " AHEAD: " + lastFrontReading);
		//System.out.println("NUM INTERSECTIONS = " + path.size());
		boolean left = false;
		boolean right = false;
		boolean ahead = false;
		if(lastLeftReading > SIDE_THRESHOLD){ways++; left = true;}
		if(lastRightReading > SIDE_THRESHOLD){ ways++; right = true;}
		if(lastFrontReading > 2*INTERSECTION_WIDTH -4){ways++; ahead = true;}
		return new Intersection(ways, directionFacing, left, ahead, right);
	}

	public void drive(Robot r){
		System.out.println("<drive/>");
		double d = lastLeftReading - lastRightReading;
		if(Math.abs(d) < 10){}
		else if(d < 0)
			r.turnRight(ADJUST_ANGLE);
		else if(d > 0)
			r.turnLeft(ADJUST_ANGLE);
		r.goForward();
		r.waitDistance(DRIVE_DISTANCE);
		driving = true;
		//orient robot parallel to the wall
		//if LEFT_SENSOR_DISTANCE < RIGHT_SENSOR_DISTANCE
		//point robot toward right side with the turn's magnitude 
		//determined by the the difference between the distances
		//else if RIGHT_SENSOR_DISTANCE < LEFT_SENSOR_DISTANCE
		//point robot toward left side with the turn's magnitude 
		//determined by the the difference between the distances
		//drive for a certain DISTANCE
	}

	public void turnAround(Robot r){
		//turn 180 --> change heading
		r.turnLeft();
		r.waitAngle(180);	
		directionFacing = Intersection.getReverse(directionFacing);
		//isRetracing = true;
	}

	public void turnToFace(Robot r, int direction){
		System.out.println("TURNINGTOFACE: " + Intersection.getString(direction) 
				+ " FROM " + Intersection.getString(directionFacing));
		if(direction == directionFacing)return;
		int right = Intersection.getRight(directionFacing);
		int left = Intersection.getLeft(directionFacing);
		int reverse = Intersection.getReverse(directionFacing);
		if(right == direction){
			System.out.println("RIGHT TURN");
			r.turnRight();
			r.waitAngle(90);
		}else if(left==direction){
			System.out.println("LEFT TURN");
			r.turnLeft();
			r.waitAngle(90);
		}else if(reverse == direction){
			//System.out.println("180 TURN");
			r.turnRight();
			r.waitAngle(180);
		}
		directionFacing = direction;
	}

	private void chooseDirection(Robot r, Intersection inter){
		//select direction that is both open and not tried then choose that path
		if(inter.getWays() > 1){
			int i = this.path[currentWaypoint];
			currentWaypoint++;
			System.out.println("ON INTERSECTION....\n" +inter.toString());
			inter.setDirectionTried(i, true);
			inter.setWent(i);
			System.out.println("CHOSE DIRECTION: " + i);				
			r.goForward();
			int enterDist =(int)((driving? 20/1.4142 : 0) + INTERSECTION_WIDTH/2);
			int exitDist = INTERSECTION_WIDTH/2;
			System.out.println("<entering>" +driving +" " +enterDist+"</entering>");
			r.waitDistance(enterDist);
			turnToFace(r, i);
			r.goForward();
			System.out.println("<exiting>" +exitDist +"</exiting>");
			r.waitDistance(exitDist);
		}else{
			for(int i = 0; i<4; i++){
				//for(int i = 3; i >= 0; i--){
				if(inter.getOpenDirections()[i] && !inter.getDirectionsTried()[i]){
					System.out.println("ON INTERSECTION....\n" +inter.toString());
					inter.setDirectionTried(i, true);
					inter.setWent(i);
					System.out.println("CHOSE DIRECTION: " + i);				
					r.goForward();
					int enterDist =(int)((driving? 20/1.4142 : 0) + INTERSECTION_WIDTH/2);
					int exitDist = INTERSECTION_WIDTH/2;
					System.out.println("<entering>" +driving +" " +enterDist+"</entering>");
					r.waitDistance(enterDist);
					turnToFace(r, i);
					r.goForward();
					System.out.println("<exiting>" +exitDist +"</exiting>");
					r.waitDistance(exitDist);
					//try{ Thread.sleep(1000);}catch(Exception e){}
					break;
				}
				System.out.println("NO LINE ON THE HORIZON: exit -1");
			}
		}
	}

	public void updateSensors(Sensor[] sensors) {
		lastFrontReading = sensors[1].getState()[0];
		lastLeftReading = sensors[4].getState()[0];
		lastRightReading = sensors[2].getState()[0];
		atIntersection = lastFrontReading < FRONT_THRESHOLD ||
		lastLeftReading >= SIDE_THRESHOLD ||
		lastRightReading >= SIDE_THRESHOLD;
		if(atIntersection)
			triggerCount++;
		else 
			triggerCount=0;
	}

}
